//Other
#include "./graph/core/graph_optimizer_sparse.cpp"
#include "./graph/core/hyper_graph.h"
#include "./graph/core/block_solver.h"
#include "./graph/core/solver.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/math_groups/se3quat.h"
#include "./graph/core/structure_only_solver.h"
#include "./graph/vertex_se3_quat.cpp"
#include "./graph/edge_se3_quat.cpp"

#include "Map.h"
#include "Transformation.cpp"
#include "FrameNode.cpp"

#include "FrameMatcher.cpp"
#include "BasicIcpMatcher.cpp"
#include "SurfIcpMatcherI.cpp"
#include "SurfIcpMatcherII.cpp"
#include "SurfRansacMatcherI.cpp"
#include <string>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/passthrough.h>

#include <Eigen/Core>
#include <fstream>

using namespace std;
void setCol(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, int r, int g, int b);
double largest_weight = 0;

Map::Map()
{
	first= 0;
	last = 0;
	nr_frames = 0;
	graphoptimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
	graphoptimizer.setVerbose(true);
	g2o::BlockSolver_6_3::LinearSolverType * linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
	g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(&graphoptimizer,linearSolver);
	graphoptimizer.setSolver(solver_ptr);
	g2o::SparseOptimizer * gr = startNewGraph();
	matcher = new SurfRansacMatcherI();
	//matcher = new BasicIcpMatcher();

	printf("-->created map<--\n");
}

Map::~Map(){}
const bool debugg_addFrame = true;
bool Map::addFrame(RGBDFrame * frame)
{

	FrameNode * current = new FrameNode();
	current->matrix = Eigen::Matrix4f::Identity();
	current->frame = frame;
	current->next = 0;
	current->prev = last;
	current->transformation = 0;
	current->locked = false;
	current->id = nr_frames;
	int max_backing = 10;
	if(nr_frames == 0){
		first = current;
		if(debugg_addFrame){printf("first...\n");}
	}
	else{
		if(debugg_addFrame){printf("not first...\n");}
		FrameNode * currentTmp2 = current;
		Transformation * transformationTmp;
		bool added = false;
		for(int i = 0; i < min(nr_frames,max_backing); i++)
		{
			currentTmp2 = currentTmp2->prev;
			Transformation * trans2 = matcher->getTransformation(current,currentTmp2);
			if(trans2->weight > 500){
				if(debugg_addFrame){printf("Added edge: %i -> %i\n",current->id,currentTmp2->id);}
				added = true;
				if(i == 0 || current->transformation == 0){current->transformation = trans2;}
				else{transformationTmp->next = trans2;}
				transformationTmp = trans2;
			}else{
				if(debugg_addFrame){printf("Delete transformation should be done here\n");}
			}
		}

		if(!added)
		{
			printf("didnt add anything, adding dummy \n");
			Transformation * transformation_dummy = new Transformation();
			transformation_dummy->src = current;
			transformation_dummy->dst = current->prev;
			transformation_dummy->transformationMatrix = Eigen::Matrix4f::Identity();
			transformation_dummy->weight = 100;
			transformation_dummy->level = 0;
			current->transformation = transformation_dummy;
		}
	}

	addNode(current);

	if(nr_frames > 0)
	{	
		FrameNode * currentTmp2 = current;
		int backwards_counter = 0;
		FrameNode * mid = 0;
		
		while(currentTmp2!=0 && backwards_counter < 2*max_backing+1)
		{
			if(backwards_counter == max_backing || (currentTmp2->prev == 0 && backwards_counter < max_backing)){mid = currentTmp2;}
			currentTmp2 = currentTmp2->prev;
			backwards_counter++;
		}
		if(mid != first){
			Transformation * trans2 = getSolidTransformation(mid, max_backing);
			trans2->next = mid->transformation;
			if(mid->transformation != 0){mid->transformation->prev = trans2;}
			mid->transformation = trans2;
		}

		last->next = current;
		last = current;
		resetGraph();		
	}
	else{last = current;}
	nr_frames++;
/*
	if(nr_frames >= 2)
	{
		FrameNode * compareTmp = current->prev;
		while(compareTmp != 0)
		{
			frame_similarity[current->frame->id_number][compareTmp->frame->id_number] = current->frame->compareToFrame(compareTmp->frame);
			frame_similarity[compareTmp->frame->id_number][current->frame->id_number] = frame_similarity[current->frame->id_number][compareTmp->frame->id_number];
			//printf("%i to %i -> %f\n",current->frame->id_number,compareTmp->frame->id_number,frame_similarity[current->frame->id_number][compareTmp->frame->id_number]);
			compareTmp = compareTmp->prev;
		}
	}
	double lol4 = get_time_now();
	printf("Total comparison time: %f\n",lol4-lol3);
	
	int start_index = first->frame->id_number;
	
	float max_dissimilar = -1;
	for(int i = 0; i < nr_frames-1;i++)
	{
		
		if(frame_similarity[start_index+i][current->frame->id_number] > max_dissimilar)
		{
			max_dissimilar = frame_similarity[start_index+i][current->frame->id_number];
		}
	}
	max_dissimilar*=1.1;
	
	float similarity_sum = 0;
	for(int i = 0; i < nr_frames-1;i++)
	{
		//printf("%f\n",frame_similarity[start_index+i][current->frame->id_number]);
		similarity_sum+=max_dissimilar-frame_similarity[start_index+i][current->frame->id_number];
	}
	printf("sum: %f\n",similarity_sum);
	int number_of_random_values = 5;
	float * random_values = new float[number_of_random_values];
	random_values[0] = ((float)rand())/((float)RAND_MAX);
	for(int i = 1; i < number_of_random_values; i++)
	{
		float random_value = ((float)rand())/((float)RAND_MAX);
		int j = i;
		while(true)
		{
			if(random_values[j-1] < random_value)
			{
				random_values[j] = random_value;
				break;
			}
			random_values[j] = random_values[j-1];
			j--;
			if(j == 0)
			{
				random_values[0] = random_value;
				break;
			}
		}
	}
	float cumu_sum = 0;
	int random_counter = 0;
	int * selections = new int[number_of_random_values];
	FrameNode * frame_node_tmp = first;
	for(int i = 0; i < nr_frames-1;i++)
	{
		cumu_sum+=max_dissimilar-frame_similarity[start_index+i][current->frame->id_number];
		if(cumu_sum > random_values[random_counter] * similarity_sum && random_counter < number_of_random_values)
		{
			selections[random_counter] = i;
			random_counter++;
			Transformation * trans2 = getTransformationFast(current,frame_node_tmp);
			printf("matching score: %f\n",trans2->weight);
			if(trans2->weight > 500 && current->frame->id_number - frame_node_tmp->frame->id_number > 10){
				printf("lala...\n");
				vizualizeTransformation(trans2);
				cvWaitKey(0);
			}
		}
		frame_node_tmp = frame_node_tmp->next;
	}
	
//	printf("_____________________________________________\n");
//	for(int i = 0; i < random_counter; i++){printf("%i\n",selections[i]);}
//	delete selections;
	
	delete random_values;
	*/
/*	
	if(nr_frames % 4 == 0)
	{
		float max = 0;
		for(int i = 0; i < nr_frames;i++)
		{
			for(int j = 0; j < nr_frames;j++)
			{
				if(frame_similarity[first->frame->id_number+i][first->frame->id_number+j]>max)
				{
					max = frame_similarity[first->frame->id_number+i][first->frame->id_number+j];
				}
			}
		}
		IplImage * tmp_image = 0;
		tmp_image = cvCreateImage(cvSize(nr_frames,nr_frames), IPL_DEPTH_8U, 3);
		for(int i = 0; i < nr_frames;i++)
		{
			for(int j = 0; j < nr_frames;j++)
			{
				if(frame_similarity[first->frame->id_number+i][first->frame->id_number+j] > 0)
				{
					tmp_image->imageData[3 * (j * nr_frames + i) + 0] = 255.0f * frame_similarity[first->frame->id_number+i][first->frame->id_number+j] / max;
					tmp_image->imageData[3 * (j * nr_frames + i) + 1] = 255.0f * frame_similarity[first->frame->id_number+i][first->frame->id_number+j] / max;
					tmp_image->imageData[3 * (j * nr_frames + i) + 2] = 255.0f * frame_similarity[first->frame->id_number+i][first->frame->id_number+j] / max;
				}else
				{
					tmp_image->imageData[3 * (j * nr_frames + i) + 0] = 0;
					tmp_image->imageData[3 * (j * nr_frames + i) + 1] = 0;
					tmp_image->imageData[3 * (j * nr_frames + i) + 2] = 0;
				}
			}
		}
		// Create an image for the output
		IplImage* tmp_image2 = cvCreateImage( cvGetSize(tmp_image), IPL_DEPTH_8U, 3 );
		cvSmooth( tmp_image, tmp_image2, CV_GAUSSIAN, 11, 11 );
		
		cvNamedWindow("tmp_image", CV_WINDOW_AUTOSIZE);
		cvShowImage("tmp_image", tmp_image);
		
		cvNamedWindow("tmp_image2", CV_WINDOW_AUTOSIZE);
		cvShowImage("tmp_image2", tmp_image2);
		cvWaitKey(30);
		cvReleaseImage(&tmp_image);
		cvReleaseImage(&tmp_image2);
	}
	printf("fit_of_frame: %f\n", fit_of_frame/nr_frames);
*/
	return true;
}
g2o::SparseOptimizer * Map::startNewGraph()
{
	g2o::SparseOptimizer * graph = new g2o::SparseOptimizer;
	graph->setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
	graph->setVerbose(false);
	g2o::BlockSolver_6_3::LinearSolverType * linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
	g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(&graphoptimizer,linearSolver);
	graph->setSolver(solver_ptr);
	return graph;
}

g2o::EdgeSE3 * Map::getG2OEdge(Transformation * transformation)
{
	Eigen::Affine3f eigenTransform(transformation->transformationMatrix);
	Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
	g2o::SE3Quat transfoSE3(
			Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),
			Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
	g2o::EdgeSE3* edgeSE3 = new g2o::EdgeSE3;	
	edgeSE3->vertices()[0] = graphoptimizer.vertex(transformation->src->id);
	edgeSE3->vertices()[1] = graphoptimizer.vertex(transformation->dst->id);
	edgeSE3->setMeasurement(transfoSE3.inverse());
	edgeSE3->setInverseMeasurement(transfoSE3);
	Eigen::Matrix<double, 6, 6, 0, 6, 6> mat;
	mat.setIdentity(6,6);
	edgeSE3->information() = mat;
	edgeSE3->weight = transformation->weight;
	return edgeSE3;
}

g2o::VertexSE3 * Map::getG2OVertex(FrameNode * node)
{
	Eigen::Affine3f eigenTransform(node->matrix);
	Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
	g2o::SE3Quat poseSE3(
		Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),
		Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
	g2o::VertexSE3 *vertexSE3 = new g2o::VertexSE3();
	vertexSE3->setId(node->id);
	vertexSE3->estimate() = poseSE3;
	return vertexSE3;
}

void Map::addTransformation(Transformation * transformation)
{
	transformation->g2oEdge = getG2OEdge(transformation);
	graphoptimizer.addEdge(transformation->g2oEdge);
}

void Map::addNode(FrameNode * node)
{
	node->g2oVertex = getG2OVertex(node);
	if(node==first || node->locked){
		node->g2oVertex->setFixed(true);
	}
	graphoptimizer.addVertex(node->g2oVertex);
}

void Map::clearGraph()
{
	FrameNode * current = first;
	while(current != 0)
	{
		Transformation * trans = current->transformation;
		while(trans!=0)
		{
			graphoptimizer.removeEdge(trans->g2oEdge);
			trans = trans->next;
		}
		graphoptimizer.removeVertex(current->g2oVertex);
		current = current->next;
	}
}

void Map::resetGraph()
{
	clearGraph();

	FrameNode * current;
	Transformation * possible_transformation;

	current = first;
	while(current !=0)
	{
		current->locked = false;
		addNode(current);
		current->locked = false;
		current = current->next;
	}
	
	current = first;
	while(current !=0)
	{
		possible_transformation = current->transformation;
		bool added = false;
		while(possible_transformation!=0)
		{
			if(possible_transformation->level == 2){
				//printf("level %i = %i <--> %i\n", possible_transformation->level, possible_transformation->src->id,possible_transformation->dst->id);
				addTransformation(possible_transformation);
				added = true;
			}
			possible_transformation = possible_transformation->next;
		}
		
		if(!added)
		{
			possible_transformation = current->transformation;
			while(possible_transformation!=0)
			{
				if(possible_transformation->level != 2){
					//printf("level %i = %i <--> %i\n", possible_transformation->level, possible_transformation->src->id,possible_transformation->dst->id);
					addTransformation(possible_transformation);
				}
				possible_transformation = possible_transformation->next;
			}
		}
		current = current->next;
	}

	optimizeGraph();
	
	current = first;	
	while(current!= 0)
	{
		current->matrix = (((g2o::VertexSE3*)(graphoptimizer.vertex(current->id)))->estimate().to_homogenious_matrix()).cast<float>();		
		//Eigen::Vector4f vect = current->matrix*Eigen::Vector4f(current->frame->x_mid,current->frame->y_mid,current->frame->z_mid,1);
		//current->x = vect(0);
		//current->y = vect(1);
		//current->z = vect(2);
			
		current = current->next;
	}
}

void Map::optimizeGraph()
{
	//printf("graph optimization\n");
	double t1 = get_time_now();
	graphoptimizer.initializeOptimization();
	graphoptimizer.setVerbose(true);
	graphoptimizer.optimize(10);
	double t2 = get_time_now();
	printf("optimizeGraph time consumptions: %f\n",t2-t1);
}
using namespace std;

void Map::setViewer(pcl::visualization::CloudViewer * viewer){printf("loels\n");}

void Map::load(string path)
{
	pcl::visualization::CloudViewer viewer("Cloud Viewer");
	view = &viewer;
	
	char buf [1024];
	printf("in file: %s\n",path.c_str());
	RGBDCamera * cam = new RGBDCamera();
	sprintf (buf, "%s.camera", path.c_str());
	cam->load(string(buf));
	this->camera = cam;

	ifstream infile (path.c_str(),ifstream::binary);
	infile.seekg(0,ifstream::end);
	long size=infile.tellg();
	char * buffer_char 		= new char [size];
	int * buffer_int 		= (int *) buffer_char;
	float * buffer_float	= (float *) buffer_char;
	infile.seekg(0);
	infile.read (buffer_char,size);
	infile.close();
	printf("size: %i nr_frames: %i\n",size,buffer_int[0]);
	nr_frames = buffer_int[0];
	FrameNode ** nodes = new FrameNode*[nr_frames];
	for(int i = 0; i < nr_frames; i++)
	{

		sprintf (buf, "%s.node_%i", path.c_str(),i);
		printf("loading %s\n",buf);
		FrameNode * current = new FrameNode();
		nodes[i] = current;
		current->load(string(buf),cam,nodes);
		printf("done loading %s\n",buf);
	}
	FrameNode * current = nodes[0];
	first = current;
	first->prev = 0;
	last = nodes[nr_frames-1];
	last->next = 0;
	for(int i = 1; i < nr_frames; i++)
	{
		nodes[i]->prev = nodes[i-1];
		nodes[i-1]->next = nodes[i];
	}
	printf("done loading map\n");
	delete buffer_char;
	
	for(int i = 0; i < nr_frames; i++)
	{
		FrameNode * node = nodes[i];
		Transformation * trans = node->transformation;
		while(trans != 0)
		{
			view->showCloud(vizualizeTransformation(trans));
			cvWaitKey(0);
			trans = trans->next;
		}
	}
}

void Map::store(string path)
{
	printf("Map out file: %s\n",path.c_str());
	long size = sizeof(nr_frames);
	char * buffer_char 		= new char[size];
	int * buffer_int 		= (int *) buffer_char;
	float * buffer_float 	= (float *) buffer_char;
	buffer_int[0] = nr_frames;
	ofstream outfile (path.c_str(),ofstream::binary);
	outfile.write (buffer_char,size);
	outfile.close();
	FrameNode * current = first;
		
	char buf [1024];
	sprintf (buf, "%s.camera", path.c_str());
	camera->store(string(buf));
	//exit(0);
	while(current != NULL)
	{
		sprintf (buf, "%s.node_%i", path.c_str(),current->id);
		current->store(string(buf));
		current = current->next;
	}
}

void Map::slam_once(RGBDCamera * camera)
{
	this->camera = camera;
	printf("slam once\n");
	pcl::visualization::CloudViewer viewer("Cloud Viewer");
	view = &viewer;
	int start = 100;
	camera->current_frame_number = start;
	int step = 1;
	const int max_iter = 10;//camera->nr_frames;
	while(camera->current_frame_number < min(start+step*max_iter,camera->nr_frames))
	{
		double t1 = get_time_now();
		camera->current_frame_number += step;
		RGBDFrame * frame = camera->getFrame(camera->current_frame_number);
		
		FrameNode * current = new FrameNode();
		current->matrix = Eigen::Matrix4f::Identity();
		current->frame = frame;
		current->next = 0;
		current->prev = last;
		current->transformation = 0;
		current->locked = false;
		current->id = nr_frames;
		addNode(current);
		int max_backing = 30;
		if(nr_frames == 0){
			first = current;
			printf("first...\n");
		}
		else{
			{printf("not first...\n");}
			FrameNode * currentTmp2 = current;
			Transformation * transformationTmp;
			bool added = false;
			for(int i = 0; i < min(nr_frames,max_backing); i++)
			{
				currentTmp2 = currentTmp2->prev;
				Transformation * trans2 = matcher->getTransformation(current,currentTmp2);
				//view->showCloud(vizualizeTransformation(trans2));
				//cvWaitKey(0);
				//trans2->display();


				if(trans2->weight > 500){
					/*printf("________________________________________________________________________\n");
					printf("Added edge: %i -> %i\n",current->id,currentTmp2->id);
					for(int ii = 0; ii < 4; ii++)
					{
						for(int jj = 0; jj < 4; jj++)
						{
							printf("%f , ",trans2->transformationMatrix(ii,jj));
						}
						printf("\n");
					}
					printf("________________________________________________________________________\n");
					addTransformation(trans2);*/
					added = true;
					if(i == 0 || current->transformation == 0){current->transformation = trans2;}
					else{transformationTmp->next = trans2;}
					transformationTmp = trans2;
				}else{
					printf("Delete transformation should be done here\n");
				}
			}

			if(!added)
			{
				printf("didnt add anything, adding dummy \n");
				Transformation * transformation_dummy = new Transformation();
				transformation_dummy->src = current;
				transformation_dummy->dst = current->prev;
				transformation_dummy->transformationMatrix = Eigen::Matrix4f::Identity();
				transformation_dummy->weight = 100;
				transformation_dummy->level = 0;
				current->transformation = transformation_dummy;
				addTransformation(transformation_dummy);
			}
			last->next = current;
		}
		//last->next = current;
		//current->prev = last;
		last = current;
		nr_frames++;
		double t2 = get_time_now();
		printf("iteration time: %f\n",t2-t1);
	}
	resetGraph();
	optimizeGraph();
	store("./store/loltest");
	load ("./store/loltest");
	exit(0);
	//printf("done\n");
	//view->showCloud(getMapCloud());
	//printf("done..\n");
	//sleep(100);
}

void Map::slam(RGBDCamera * camera)
{
	this->camera = camera;
	pcl::visualization::CloudViewer viewer("Cloud Viewer");
	view = &viewer;
	int start = 100;
	camera->current_frame_number = start;
	int step = 1;
	const int max_iter = camera->nr_frames;
	
	frame_similarity	= new double * [max_iter];
	for(int i = 0; i < max_iter; i++){
		frame_similarity[i]	= new double[max_iter];
		for(int j = 0; j < max_iter; j++){
			frame_similarity[i][j]	= -1;
		}
	}
	while(camera->current_frame_number < min(start+step*max_iter,camera->nr_frames))
	{
		camera->current_frame_number += step;
		RGBDFrame * current_frame = camera->getFrame(camera->current_frame_number);
		bool success = addFrame(current_frame);
		if(camera->current_frame_number % 20 == 0)
		{
			view->showCloud(getMapCloud());
			//sleep(5);
		}
	}
	//view.showCloud(vizualizeClusters());
	while(true){usleep(1000);}
}

void Map::optimizeTransformation(RGBDCamera * camera)
{
	pcl::visualization::CloudViewer viewer("Cloud Viewer");
	view = &viewer;

	printf("optimizeTransformation\n");
	//(matcher)->map = this;
	const unsigned int max = 150;
	FrameNode ** frames = new FrameNode *[max];

	for(int i = 0; i < max; i++)
	{
		printf("count:%i\n",i);
		RGBDFrame * frame =  camera->getNextFrame();
		FrameNode * current = new FrameNode();
		current->matrix = Eigen::Matrix4f::Identity();
		current->frame = frame;
		current->next = 0;
		current->prev = 0;
		current->transformation = 0;
		current->locked = false;
		current->id = i;
		frames[i] = current;
	}

	int source = 100;
	int dest = 1;
	cvNamedWindow("source image", CV_WINDOW_AUTOSIZE );
	cvNamedWindow("destination image", CV_WINDOW_AUTOSIZE );

	while(true)
	{
		IplImage * source_img 	= cvLoadImage( frames[source]->frame->rgb_path.c_str(), CV_LOAD_IMAGE_COLOR );
		IplImage * dest_img 	= cvLoadImage( frames[dest]->frame->rgb_path.c_str(), CV_LOAD_IMAGE_COLOR );
		cvShowImage("source image", source_img);
		cvShowImage("destination image", dest_img);
		int key = cvWaitKey(0);
		printf("source: %i dest: %i key: %i\n",source,dest,key);
		cvReleaseImage( &source_img );
		cvReleaseImage( &dest_img );
		switch(key)
		{
			case 1048624 : {
				printf("0\n");
				break;
			}
			case 1048625 : {
				printf("1\n");
				if(source < max - 2){
					source++;
				}
				break;
			}
			case 1048626 : {
				printf("2\n");
				if(source > 0){
					source--;
				}
				break;
			}
			case 1048627 : {
				if(dest < max - 2){
					dest++;
				}
				printf("3\n");
				break;
			}
			case 1048628 : {
				printf("4\n");
				if(dest > 0){
					dest--;
				}
				break;
			}
			case 1048629 : {
				printf("5\n");
				Transformation * found_transformation = matcher->getTransformation(frames[source],frames[dest]);
				found_transformation->display();
				viewer.showCloud(vizualizeTransformation(found_transformation));
				cvWaitKey(0);
				printf("done\n");			
				break;
			}
			case 1048630 : {
				printf("6\n");

				break;
			}
			case 1048631 : {
				printf("7\n");
				break;
			}
			case 1048632 : {
				printf("8\n");
				break;
			}
			case 1048633 : {
				printf("9\n");
				break;
			}
			case 1048689 : {
				printf("q\n");
				break;
			}
			case 1048695 : {
				printf("w\n");
				break;
			}
			case 1048677 : {
				printf("e\n");
				break;
			}
			case 1048690 : {
				printf("r\n");
				break;
			}
			case 1048692 : {
				printf("t\n");
				break;
			}
			case 1048697 : {
				printf("y\n");
				break;
			}
			case 1048693 : {
				printf("u\n");
				break;
			}
			case 1048681 : {
				printf("i\n");
				break;
			}
			case 1048687 : {
				printf("o\n");
				break;
			}
			case 1048688 : {
				printf("p\n");
				break;
			}
		}
	}
	exit(0);
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr  Map::getMapCloud()
{
	printf("vizualizeMap\n");
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr full_cloud 	(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr result2 		(new pcl::PointCloud<pcl::PointXYZRGB>);
	FrameNode * current = first;
	while(current != NULL)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr current_cloud = current->frame->getCloud();
		printf("________________________________________________________________________\n");
		for(int i = 0; i < 4; i++)
		{
			for(int j = 0; j < 4; j++)
			{
				printf("%f , ",current->matrix(i,j));
			}
			printf("\n");
		}
		pcl::transformPointCloud (*current_cloud, *result2, current->matrix);
		//*full_cloud += *result2;
		current = current->next;
		printf("iter\n");
	}
	//exit(0);
	return full_cloud;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr  Map::vizualizeTransformation(Transformation * transformation)
{
	printf("vizualizeTransformation\n");

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr full_cloud 	(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr result2 		(new pcl::PointCloud<pcl::PointXYZRGB>);

	pcl::transformPointCloud (*(transformation->src->frame->getCloud()), *result2, transformation->transformationMatrix);
	printf("width cloud1: %i\n",result2->width);
	setCol(result2, 0, 255, 0);
	/*
	for(int i = 0; i < result2->width; i++)
	{
		result2->points[i].r = 0;
		result2->points[i].g = 255;
		result2->points[i].b = 0;
	}
	*/
	*full_cloud += *result2;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = transformation->dst->frame->getCloud();
	setCol(tmp, 255, 0, 0);
	/*
	printf("width cloud2: %i\n",tmp->width);
	for(int i = 0; i < tmp->width; i++)
	{
		tmp->points[i].r = 255;
		tmp->points[i].g = 0;
		tmp->points[i].b = 0;
	}*/
	*full_cloud += *tmp;
	//view->showCloud(full_cloud);
	printf("done vizualizing cloud\n");
	return full_cloud;
}

Transformation * Map::getSolidTransformation(FrameNode * mid,int offset)
{
	clearGraph();

	FrameNode * first_node = mid;
	FrameNode * last_node  = mid;

	int counter = 0;
	FrameNode * current = mid->next;
	Transformation * possible_transformation;	

	while(current !=0 && counter < offset)
	{
		first_node = current;		
		current->locked = false;
		addNode(current);
		current = current->next;
		counter++;
	}

	addNode(mid);

	current = mid->prev;
	counter = 0;
	while(current !=0 && counter < offset)
	{
		current->locked = true;
		addNode(current);
		current->locked = false;
		
		last_node = current;
		current = current->prev;
		counter++;
	}
	
	counter = 0;
	current = mid->next;

	while(current !=0 && counter < offset)
	{
		possible_transformation = current->transformation;
		while(possible_transformation!=0)
		{
			addTransformation(possible_transformation);
			possible_transformation = possible_transformation->next;
		}
		current = current->next;
		counter++;
	}
	
	possible_transformation = mid->transformation;
	while(possible_transformation!=0)
	{
		addTransformation(possible_transformation);
		possible_transformation = possible_transformation->next;
	}

	printf("start transofrmation optimization\n");
	optimizeGraph();
	printf("stop transofrmation optimization\n");
	
	current = last_node;	
	while(current!= 0 && current->id <= first_node->id)
	{
		g2o::VertexSE3 * vertexSE3 = (g2o::VertexSE3*)(graphoptimizer.vertex(current->id));
		Eigen::Matrix4f transformation_matrix = (vertexSE3->estimate().to_homogenious_matrix()).cast<float>();
		current = current->next;
	}
	
	Transformation * new_transformation = new Transformation();
	new_transformation->src = mid;
	new_transformation->dst = mid->prev;
	new_transformation->weight = 1337;
	new_transformation->level = 2;
	new_transformation->transformationMatrix = Eigen::Matrix4f::Identity();

	g2o::VertexSE3 * vertexSE3_src = (g2o::VertexSE3*)(graphoptimizer.vertex(new_transformation->src->id));
	Eigen::Matrix4f transformation_matrix_src = (vertexSE3_src->estimate().to_homogenious_matrix()).cast<float>();
	g2o::VertexSE3 * vertexSE3_dst = (g2o::VertexSE3*)(graphoptimizer.vertex(new_transformation->dst->id));
	Eigen::Matrix4f transformation_matrix_dst = (vertexSE3_dst->estimate().to_homogenious_matrix()).cast<float>();
	new_transformation->transformationMatrix = transformation_matrix_src * transformation_matrix_dst.inverse();

	return new_transformation;
}

void setCol(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, int r, int g, int b)
{
	printf("setting col: %i %i %i\n",r,g,b);
	for (int index = 0; index < cloud->width; index++)
	{
		cloud->points[index].r = r;
		cloud->points[index].g = g;
		cloud->points[index].b = b;
	}
}
